#include <stdio.h>

#include "line_algorithm.h"
#define TAG "LINE_ALGO"

control_type_t get_line_and_gyro_err(uint8_t *pos, float *_err) {
  int8_t err = 0;
  int left_1 = -1, right_1 = -1;
  const static int8_t weight[8] = {-9, -6, -3, -1, 1, 3, 6, 9};

  for (int i = 0; i < 8; i++) {
    if (pos[i] == 1) {
      left_1 = i;
      break;
    }
  }
  for (int i = 7; i >= 0; i--) {
    if (pos[i] == 1) {
      right_1 = i;
      break;
    }
  }
  for (int i = left_1; i <= right_1; i++) {
    if (left_1 == -1 && right_1 == -1) {
      *_err = 0;
      return CONTROL_TYPE_GYRO;
    }
    err += weight[i] * pos[i];

    if (my_abs(err) <= 1)
      err = 0;

    *_err = (float)err;

    return CONTROL_TYPE_LINE;
  }

  return CONTROL_TYPE_LINE;
}
